# pragma once

# include "cpp_tools.h"
# include "udpSettings.h"
# include "dataStruct.h"
# include "tracking_base.h"

class mpc_controller : public tracking_base
{
protected:
    // 求解部分
    cpp_tools::udpVecServer<int64_t> udp2solver;
    
    struct status
    {
        cpp_tools::mtDataManager<bool> solver_busy = false; // 当前是否正在对求解器进行操作

        cpp_tools::mtDataManager<int> refresh = 0; // 0：没有进行该操作；-1：失败；1：成功

        cpp_tools::mtDataManager<double> computTime = -1; // 计算时间（包括数据传输时间）, ms
    } m_status;
    
    cpp_tools::mtDataManager<std::thread>  thread_solver_operation; // 求解器操作线程

    struct controlInterp
    {
        cpp_tools::gridInterp delta_interp;
        cpp_tools::gridInterp F_x_interp;

        bool interpReady = 0;
        double baseTime = -1; // 更新时间
    };
    cpp_tools::mtDataManager<controlInterp> m_controlInterp;

    Eigen::VectorXd P_ctrl;
    Eigen::VectorXd P_modi;
    Eigen::VectorXd P_art;

    const double pi = cpp_tools::pi; // 圆周率

    /// @brief 使用mpc算法进行一次计算
    /// @param linCurve Frenet坐标
    /// @param dynamic 速度、加速度
    /// @param delta_now 当前输出的转角
    /// @param F_x_now 当前输出的纵向力
    /// @return 0：完成计算；-1：错误的输入状态值；-2：求解超时；-3：求解结果错误；-4：插值错误
    int mpcComput(data::Frenet linCurve, data::Dynamic dynamic, double delta_now, double F_x_now)
    { 
        int N = 20;

        Eigen::VectorXd dS = dynamic.v*0.05*Eigen::VectorXd::Ones(N);
        double S_start = linCurve.s;
        Eigen::VectorXd S_grid = cpp_tools::cumtrapz(dS).array() + S_start;
        if (loop)
        {
            double s_go = m_param.m_track.S(0);
            double s_end = m_param.m_track.S(Eigen::last);
            for (auto &&s : S_grid)
            {
                while (1)
                {
                    if (s < s_go)
                    {
                        s += (s_end - s_go);
                    }
                    else if (s > s_end)
                    {
                        s -= (s_end - s_go);
                    }
                    else
                    {
                        break;
                    }
                }
            }
        }

        double B = m_param.m_veh.m_tire.MF_y.B;
        double C = m_param.m_veh.m_tire.MF_y.C;
        double D = m_param.m_veh.m_tire.MF_y.D;
        double E = m_param.m_veh.m_tire.MF_y.E;
        auto fun_f = [B,C,D,E](double s) -> double {
            return -D*std::sin(C*std::atan(s*B - E*(s*B - std::atan(s*B))));
        };
        auto [s_y_peak, val, iter] = cpp_tools::goldenSection(fun_f,0.1,0.5,1e-8,1e-8,40);

        Eigen::VectorXd P_sys(19);
        P_sys << 
            m_param.m_veh.m_body.mass_sprung,
            m_param.m_veh.m_body.mass_unsprung_front,
            m_param.m_veh.m_body.mass_unsprung_rear,
            m_param.m_veh.m_tire.mass_wheel,
            m_param.m_veh.m_common.g,
            m_param.m_veh.m_body.inertial_z,
            m_param.m_veh.m_body.l_f,
            m_param.m_veh.m_body.l_r,
            m_param.m_veh.m_body.l_y,
            m_param.m_veh.m_body.h_cg,
            m_param.m_veh.m_common.suspension_response,
            0.15e-2,
            0.05e-4,
            m_param.m_veh.m_tire.MF_x.D,
            m_param.m_veh.m_tire.MF_y.B,
            m_param.m_veh.m_tire.MF_y.C,
            m_param.m_veh.m_tire.MF_y.D,
            m_param.m_veh.m_tire.MF_y.E,
            s_y_peak - 0.0001;
        
        auto varphi = varphi_interp.interp(S_grid);
        auto D_varphi = curve_interp.interp(S_grid);
        auto D2_varphi = cpp_tools::gradient(D_varphi,S_grid);

        auto psi = psi_interp.interp(S_grid);
        auto D_psi = cpp_tools::gradient(psi,S_grid);
        auto D2_psi = cpp_tools::gradient(D_psi,S_grid);

        auto gamma = gamma_interp.interp(S_grid);
        auto D_gamma = cpp_tools::gradient(gamma,S_grid);
        auto D2_gamma = cpp_tools::gradient(D_gamma,S_grid);

        auto v_ref = v_ref_interp.interp(S_grid);
        auto beta_ref = beta_ref_interp.interp(S_grid);
        auto e_ref = e_ref_interp.interp(S_grid);
        auto Delta_phi_ref = Delta_phi_ref_interp.interp(S_grid);

        double Delta_varphi = linCurve.Delta_phi - dynamic.beta;
        if (std::abs(Delta_varphi) >= pi/2 || dynamic.beta >= pi/2)
        {
            return -1;
        }
        
        auto A_z = cpp_tools::Euler_Rotation_Z(varphi(0));
        auto A_y = cpp_tools::Euler_Rotation_Y(psi(0));
        auto A_x = cpp_tools::Euler_Rotation_X(gamma(0));
        auto A_varphi = cpp_tools::Euler_Rotation_Z(Delta_varphi);

        auto A_vb = A_z*A_y*A_x*A_varphi;
        Eigen::VectorXd e_vb_1 = A_vb(Eigen::all,0);
        Eigen::VectorXd e_vb_2 = A_vb(Eigen::all,1);

        Eigen::VectorXd vec_g(3,1);
        vec_g << 0,0,-m_param.m_veh.m_common.g;
        double load_trans_x = dynamic.a_x - vec_g.transpose()*e_vb_1;
        double load_trans_y = dynamic.a_y - vec_g.transpose()*e_vb_2;
        Eigen::VectorXd X_0(7);
        Eigen::VectorXd U_0(2);
        X_0 << dynamic.v,dynamic.beta,dynamic.omega,linCurve.e,linCurve.Delta_phi,load_trans_x,load_trans_y;
        U_0 << delta_now, F_x_now;

        int n_p = 325;
        Eigen::VectorXd p(n_p);
        p << U_0,X_0,P_sys,P_ctrl,P_modi,P_art,
            dS,
            varphi,
            D_varphi,
            D2_varphi,
            psi,
            D_psi,
            D2_psi,
            gamma,
            D_gamma,
            D2_gamma,
            v_ref,
            beta_ref,
            e_ref,
            Delta_phi_ref;

        // 发送给求解器进行计算
        cpp_tools::clock m_clock;

        Eigen::Vector<int64_t, Eigen::Dynamic> p_send(1 + n_p + 1); // 为了可视化方便，额外将s发送过去，放在参数的后面
        p_send << 
            static_cast<int64_t>(data::communicate::lower_solver::mpc_order::solve),
            (p*udpSettings::intScale).cast<int64_t>(),
            static_cast<int64_t>(S_start*udpSettings::intScale);
        udp2solver.send(p_send);

        double overTime = 1000;
        while (udp2solver.receive() < -1)
        {
            if (m_clock.toc(0) > overTime)
            {
                std::cout << "求解超时" << std::endl;

                *m_status.computTime.write().access() = -1;

                return -2;
            }
            
            std::this_thread::sleep_for(std::chrono::microseconds(100)); // 每隔0.1ms查看一次
        }
        *m_status.computTime.write().access() = m_clock.toc(0);

        // 提取结果
        Eigen::VectorXd sol_x = udp2solver.get().cast<double>()/udpSettings::intScale;
        if (sol_x.array().isNaN().any())
        {
            return -3;
        }

        auto r_opt = sol_x.topRows(260).reshaped(13,20);
        auto U_opt = r_opt.topRows(2);
        auto X_opt = r_opt.middleRows(2,7);

        Eigen::VectorXd opt_v = X_opt.row(0);
        Eigen::VectorXd opt_e = X_opt.row(3);
        Eigen::VectorXd opt_Delta_phi = X_opt.row(4);
        Eigen::VectorXd opt_dt = dS.array()*(1 - D_varphi.array()*opt_e.array())/(opt_v.array()*opt_Delta_phi.array().cos());
        
        auto opt_t = cpp_tools::cumsum(opt_dt);
        auto opt_delta = U_opt.row(0).transpose();
        auto opt_F_x = U_opt.row(1).transpose();

        Eigen::VectorXd interp_t(opt_t.size() + 1);
        Eigen::VectorXd interp_delta(opt_t.size() + 1);
        Eigen::VectorXd interp_F_x(opt_t.size() + 1);

        interp_t << 0,opt_t;
        interp_delta << delta_now,opt_delta;
        interp_F_x << F_x_now,opt_F_x;

        {
            auto ac = m_controlInterp.write();

            try
            {
                ac.access()->delta_interp.reset(interp_t,interp_delta);
                ac.access()->F_x_interp.reset(interp_t,interp_F_x);

                ac.access()->interpReady = 1;
            }
            catch(const std::exception& e)
            {
                std::cerr << e.what() << '\n';

                return -4;
            }
        }

        return 0;
    }
    
public:
    mpc_controller()
        :udp2solver(udpSettings::PORT::lower_solver::message_len,udpSettings::PORT::lower_solver::message,1),
        tracking_base()
    {
        // 读取设置
        readReference();

        // 设置order发送对象
        udp2solver.setGoal(udpSettings::IP::solver,udpSettings::PORT::lower_solver::order);
        
        {
            auto mat1 = cpp_tools::csvRead(host_path + settings_path + "/mpc/P_ctrl.csv");
            auto mat2 = cpp_tools::csvRead(host_path + settings_path + "/mpc/P_modi.csv");
            auto mat3 = cpp_tools::csvRead(host_path + settings_path + "/mpc/P_art.csv");
            
            P_ctrl.resize(5);
            P_modi.resize(9);
            P_art.resize(3);

            // v,beta,e,de,dt
            P_ctrl << 0.1,0,10,0.1,0; // 默认不考虑时间代价
            // du, dx
            P_modi << 0.001,0.001,
                0.001,0.001,0.001,0.001,0.001,0.001,0.001;
            // 人工变量罚参数（l1罚参数）
            P_art << 1e2,1e2,1e3;

            if (mat1.size() == P_ctrl.size())
            {
                P_ctrl = mat1.reshaped(P_ctrl.size(),1);
            }
            else
            {
                cpp_tools::csvWrite(host_path + settings_path + "/mpc/P_ctrl.csv",P_ctrl);
            }

            if (mat2.size() == P_modi.size())
            {
                P_modi = mat2.reshaped(P_modi.size(),1);
            }
            else
            {
                cpp_tools::csvWrite(host_path + settings_path + "/mpc/P_modi.csv",P_modi);
            }

            if (mat3.size() == P_art.size())
            {
                P_art = mat3.reshaped(P_art.size(),1);
            }
            else
            {
                cpp_tools::csvWrite(host_path + settings_path + "/mpc/P_art.csv",P_art);
            }

            // 通知当前设置
            if (P_ctrl(Eigen::last) > 0)
            {
                std::cout << "mpc controller: 使用竞速模式" << std::endl;
            }
            else
            {
                std::cout << "mpc controller: 使用常规模式" << std::endl;
            }

            // 检查参数设置是否正确
            if ((P_ctrl.array() < 0).any())
            {
                std::cout << "mpc controller: 警告, P_ctrl 中存在负值" << std::endl;
            }
            if ((P_modi.array() < 0).any())
            {
                std::cout << "mpc controller: 警告, P_modi 中存在负值" << std::endl;
            }
            if ((P_art.array() < 0).any())
            {
                std::cout << "mpc controller: 警告, P_art 中存在负值" << std::endl;
            }
        }
    };

    ~mpc_controller()
    {
        shutdown();

        std::cout << "mpc controller exit" << std::endl;
    };

    void shutdown() override
    {
        auto ac = thread_solver_operation.write();

        if (ac.access()->joinable())
        {
            ac.access()->join();
        }
    }

    /// @brief 读取状态估计的数据并进行转化
    /// @param state 
    /// @param delta 
    /// @param F_x 
    /// @param time 当前时刻 (s)
    void update(Eigen::VectorXd const & state, double delta, double F_x, double time) override
    {
        if (*m_status.solver_busy.read().access())
        {
            return ;
        }

        auto ac = thread_solver_operation.write();

        if (ac.access()->joinable())
        {
            ac.access()->join();
        }

        *ac.access() = std::move(std::thread([&,state,delta,F_x,time]() -> void {
            // 先写入求解器繁忙状态
            *m_status.solver_busy.write().access() = 1;

            // 然后通过实例化一个读取访问以禁止写入，下面的内容就是具体需要执行的操作
            {
                auto ac = m_status.solver_busy.read();

                auto [linCurve,dynamic] = readStateVec(state);
    
                int info = mpcComput(linCurve,dynamic,delta,F_x);
    
                if (info == 0)
                {
                    m_controlInterp.write().access()->baseTime = time;
                }
            }

            // 解除繁忙状态
            *m_status.solver_busy.write().access() = 0;
        }));
    }

    /// @brief 获取当前时刻的控制量
    /// @param state 
    /// @param delta 
    /// @param F_x 
    /// @param time 当前时刻 (s)
    /// @return 
    std::pair<double,double> getControl(Eigen::VectorXd const & state, double delta, double F_x, double time) override
    {
        auto ac = m_controlInterp.write();

        if (ac.access()->interpReady)
        {
            double t_now = time;
            double t_base = ac.access()->baseTime;
            const double t_forward = 0.01; // 延迟修正补偿, s

            double delta = ac.access()->delta_interp.interp(t_now - t_base + t_forward);
            double F_x = ac.access()->F_x_interp.interp(t_now - t_base + t_forward);

            return {delta,F_x};
        }
        else
        {
            return {0,0};
        }
    }

    double getComputTime() override
    {
        return *m_status.computTime.read().access();
    }

    /// @brief 刷新求解器
    /// @return 1：刷新成功；0：刷新失败（超时）
    int refresh() override
    {
        int info = 0;

        {
            auto ac_thread = thread_solver_operation.write();
    
            if (ac_thread.access()->joinable())
            {
                ac_thread.access()->join();
            }

            *ac_thread.access() = std::move(std::thread([&]() -> void {
                // 先写入求解器繁忙状态
                *m_status.solver_busy.write().access() = 1;

                // 然后通过实例化一个读取访问以禁止写入，下面的内容就是具体需要执行的操作
                {
                    auto ac = m_status.solver_busy.read();

                    Eigen::Vector<int64_t, Eigen::Dynamic> order(1);
                    order << static_cast<int64_t>(data::communicate::lower_solver::mpc_order::refresh);
                    udp2solver.send(order);

                    cpp_tools::clock m_clock;
                    double overTime = 1000; // 最多1000ms
                    while (1)
                    {
                        if (m_clock.toc(0) > overTime)
                        {
                            std::cout << "刷新超时" << std::endl;

                            *m_status.refresh.write().access() = -1;

                            break;
                        }
                        else if (udp2solver.receive() >= -1)
                        {
                            std::cout << "刷新完成" << std::endl;

                            *m_status.refresh.write().access() = 1;

                            info = 1;

                            break;
                        }
                        
                        std::this_thread::sleep_for(std::chrono::microseconds(100)); // 每隔0.1ms查看一次
                    }
                }

                // 解除繁忙状态
                *m_status.solver_busy.write().access() = 0;
            }));

            // 刷新必须等待执行完成后返回
            if (ac_thread.access()->joinable())
            {
                ac_thread.access()->join();
            }
        }

        auto ac_interp = m_controlInterp.write();
        ac_interp.access()->baseTime = -1;
        ac_interp.access()->interpReady = 0;

        return info;
    }

    mpc_controller(const mpc_controller&) = delete;
};
